/*
 * $RCSfile: Matrix3f.java,v $
 *
 * Copyright 1996-2008 Sun Microsystems, Inc.  All Rights Reserved.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * This code is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License version 2 only, as
 * published by the Free Software Foundation.  Sun designates this
 * particular file as subject to the "Classpath" exception as provided
 * by Sun in the LICENSE file that accompanied this code.
 *
 * This code is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * version 2 for more details (a copy is included in the LICENSE file that
 * accompanied this code).
 *
 * You should have received a copy of the GNU General Public License version
 * 2 along with this work; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
 * CA 95054 USA or visit www.sun.com if you need additional information or
 * have any questions.
 *
 * $Revision: 1.8 $
 * $Date: 2008/02/28 20:18:50 $
 * $State: Exp $
 */

package etrl.vecmath;

import java.lang.Math;

/**
 * A single precision floating point 3 by 3 matrix. Primarily to support 3D
 * rotations.
 * 
 */
public class Matrix3f implements java.io.Serializable, Cloneable {

	// Compatible with 1.1
	static final long serialVersionUID = 329697160112089834L;

	/**
	 * The first matrix element in the first row.
	 */
	public float m00;

	/**
	 * The second matrix element in the first row.
	 */
	public float m01;

	/**
	 * The third matrix element in the first row.
	 */
	public float m02;

	/**
	 * The first matrix element in the second row.
	 */
	public float m10;

	/**
	 * The second matrix element in the second row.
	 */
	public float m11;

	/**
	 * The third matrix element in the second row.
	 */
	public float m12;

	/**
	 * The first matrix element in the third row.
	 */
	public float m20;

	/**
	 * The second matrix element in the third row.
	 */
	public float m21;

	/**
	 * The third matrix element in the third row.
	 */
	public float m22;
	/*
	 * double[] tmp = new double[9]; // scratch matrix double[] tmp_rot = new
	 * double[9]; // scratch matrix double[] tmp_scale = new double[3]; // scratch
	 * matrix
	 */
	private static final double EPS = 1.0E-8;

	/**
	 * Constructs and initializes a Matrix3f from the specified nine values.
	 * 
	 * @param m00
	 *          the [0][0] element
	 * @param m01
	 *          the [0][1] element
	 * @param m02
	 *          the [0][2] element
	 * @param m10
	 *          the [1][0] element
	 * @param m11
	 *          the [1][1] element
	 * @param m12
	 *          the [1][2] element
	 * @param m20
	 *          the [2][0] element
	 * @param m21
	 *          the [2][1] element
	 * @param m22
	 *          the [2][2] element
	 */
	public Matrix3f(float m00, float m01, float m02, float m10, float m11,
			float m12, float m20, float m21, float m22) {
		this.m00 = m00;
		this.m01 = m01;
		this.m02 = m02;

		this.m10 = m10;
		this.m11 = m11;
		this.m12 = m12;

		this.m20 = m20;
		this.m21 = m21;
		this.m22 = m22;

	}

	/**
	 * Constructs and initializes a Matrix3f from the specified nine-element
	 * array. this.m00 =v[0], this.m01=v[1], etc.
	 * 
	 * @param v
	 *          the array of length 9 containing in order
	 */
	public Matrix3f(float[] v) {
		this.m00 = v[0];
		this.m01 = v[1];
		this.m02 = v[2];

		this.m10 = v[3];
		this.m11 = v[4];
		this.m12 = v[5];

		this.m20 = v[6];
		this.m21 = v[7];
		this.m22 = v[8];

	}

	/**
	 * Constructs a new matrix with the same values as the Matrix3d parameter.
	 * 
	 * @param m1
	 *          the source matrix
	 */
	public Matrix3f(Matrix3d m1) {
		this.m00 = (float) m1.m00;
		this.m01 = (float) m1.m01;
		this.m02 = (float) m1.m02;

		this.m10 = (float) m1.m10;
		this.m11 = (float) m1.m11;
		this.m12 = (float) m1.m12;

		this.m20 = (float) m1.m20;
		this.m21 = (float) m1.m21;
		this.m22 = (float) m1.m22;

	}

	/**
	 * Constructs a new matrix with the same values as the Matrix3f parameter.
	 * 
	 * @param m1
	 *          the source matrix
	 */
	public Matrix3f(Matrix3f m1) {
		this.m00 = m1.m00;
		this.m01 = m1.m01;
		this.m02 = m1.m02;

		this.m10 = m1.m10;
		this.m11 = m1.m11;
		this.m12 = m1.m12;

		this.m20 = m1.m20;
		this.m21 = m1.m21;
		this.m22 = m1.m22;

	}

	/**
	 * Constructs and initializes a Matrix3f to all zeros.
	 */
	public Matrix3f() {
		this.m00 = (float) 0.0;
		this.m01 = (float) 0.0;
		this.m02 = (float) 0.0;

		this.m10 = (float) 0.0;
		this.m11 = (float) 0.0;
		this.m12 = (float) 0.0;

		this.m20 = (float) 0.0;
		this.m21 = (float) 0.0;
		this.m22 = (float) 0.0;

	}

	/**
	 * Returns a string that contains the values of this Matrix3f.
	 * 
	 * @return the String representation
	 */
	public String toString() {
		return this.m00 + ", " + this.m01 + ", " + this.m02 + "\n" + this.m10
				+ ", " + this.m11 + ", " + this.m12 + "\n" + this.m20 + ", " + this.m21
				+ ", " + this.m22 + "\n";
	}

	/**
	 * Sets this Matrix3f to identity.
	 */
	public final void setIdentity() {
		this.m00 = (float) 1.0;
		this.m01 = (float) 0.0;
		this.m02 = (float) 0.0;

		this.m10 = (float) 0.0;
		this.m11 = (float) 1.0;
		this.m12 = (float) 0.0;

		this.m20 = (float) 0.0;
		this.m21 = (float) 0.0;
		this.m22 = (float) 1.0;
	}

	/**
	 * Sets the scale component of the current matrix by factoring out the current
	 * scale (by doing an SVD) and multiplying by the new scale.
	 * 
	 * @param scale
	 *          the new scale amount
	 */
	public final void setScale(float scale) {
		double[] tmp_rot = new double[9]; // scratch matrix
		double[] tmp_scale = new double[3]; // scratch matrix

		getScaleRotate(tmp_scale, tmp_rot);

		this.m00 = (float) (tmp_rot[0] * scale);
		this.m01 = (float) (tmp_rot[1] * scale);
		this.m02 = (float) (tmp_rot[2] * scale);

		this.m10 = (float) (tmp_rot[3] * scale);
		this.m11 = (float) (tmp_rot[4] * scale);
		this.m12 = (float) (tmp_rot[5] * scale);

		this.m20 = (float) (tmp_rot[6] * scale);
		this.m21 = (float) (tmp_rot[7] * scale);
		this.m22 = (float) (tmp_rot[8] * scale);

	}

	/**
	 * Sets the specified element of this matrix3f to the value provided.
	 * 
	 * @param row
	 *          the row number to be modified (zero indexed)
	 * @param column
	 *          the column number to be modified (zero indexed)
	 * @param value
	 *          the new value
	 */
	public final void setElement(int row, int column, float value) {
		switch (row) {
			case 0:
				switch (column) {
					case 0:
						this.m00 = value;
						break;
					case 1:
						this.m01 = value;
						break;
					case 2:
						this.m02 = value;
						break;
					default:
						throw new ArrayIndexOutOfBoundsException(VecMathI18N
								.getString("Matrix3f0"));
				}
				break;

			case 1:
				switch (column) {
					case 0:
						this.m10 = value;
						break;
					case 1:
						this.m11 = value;
						break;
					case 2:
						this.m12 = value;
						break;
					default:
						throw new ArrayIndexOutOfBoundsException(VecMathI18N
								.getString("Matrix3f0"));
				}
				break;

			case 2:
				switch (column) {
					case 0:
						this.m20 = value;
						break;
					case 1:
						this.m21 = value;
						break;
					case 2:
						this.m22 = value;
						break;
					default:

						throw new ArrayIndexOutOfBoundsException(VecMathI18N
								.getString("Matrix3f0"));
				}
				break;

			default:
				throw new ArrayIndexOutOfBoundsException(VecMathI18N
						.getString("Matrix3f0"));
		}
	}

	/**
	 * Copies the matrix values in the specified row into the vector parameter.
	 * 
	 * @param row
	 *          the matrix row
	 * @param v
	 *          the vector into which the matrix row values will be copied
	 */
	public final void getRow(int row, Vector3f v) {
		if (row == 0) {
			v.x = m00;
			v.y = m01;
			v.z = m02;
		} else if (row == 1) {
			v.x = m10;
			v.y = m11;
			v.z = m12;
		} else if (row == 2) {
			v.x = m20;
			v.y = m21;
			v.z = m22;
		} else {
			throw new ArrayIndexOutOfBoundsException(VecMathI18N
					.getString("Matrix3f1"));
		}

	}

	/**
	 * Copies the matrix values in the specified row into the array parameter.
	 * 
	 * @param row
	 *          the matrix row
	 * @param v
	 *          the array into which the matrix row values will be copied
	 */
	public final void getRow(int row, float v[]) {
		if (row == 0) {
			v[0] = m00;
			v[1] = m01;
			v[2] = m02;
		} else if (row == 1) {
			v[0] = m10;
			v[1] = m11;
			v[2] = m12;
		} else if (row == 2) {
			v[0] = m20;
			v[1] = m21;
			v[2] = m22;
		} else {
			throw new ArrayIndexOutOfBoundsException(VecMathI18N
					.getString("Matrix3f1"));
		}

	}

	/**
	 * Copies the matrix values in the specified column into the vector parameter.
	 * 
	 * @param column
	 *          the matrix column
	 * @param v
	 *          the vector into which the matrix row values will be copied
	 */
	public final void getColumn(int column, Vector3f v) {
		if (column == 0) {
			v.x = m00;
			v.y = m10;
			v.z = m20;
		} else if (column == 1) {
			v.x = m01;
			v.y = m11;
			v.z = m21;
		} else if (column == 2) {
			v.x = m02;
			v.y = m12;
			v.z = m22;
		} else {
			throw new ArrayIndexOutOfBoundsException(VecMathI18N
					.getString("Matrix3f3"));
		}

	}

	/**
	 * Copies the matrix values in the specified column into the array parameter.
	 * 
	 * @param column
	 *          the matrix column
	 * @param v
	 *          the array into which the matrix row values will be copied
	 */
	public final void getColumn(int column, float v[]) {
		if (column == 0) {
			v[0] = m00;
			v[1] = m10;
			v[2] = m20;
		} else if (column == 1) {
			v[0] = m01;
			v[1] = m11;
			v[2] = m21;
		} else if (column == 2) {
			v[0] = m02;
			v[1] = m12;
			v[2] = m22;
		} else {
			throw new ArrayIndexOutOfBoundsException(VecMathI18N
					.getString("Matrix3f3"));
		}
	}

	/**
	 * Retrieves the value at the specified row and column of this matrix.
	 * 
	 * @param row
	 *          the row number to be retrieved (zero indexed)
	 * @param column
	 *          the column number to be retrieved (zero indexed)
	 * @return the value at the indexed element.
	 */
	public final float getElement(int row, int column) {
		switch (row) {
			case 0:
				switch (column) {
					case 0:
						return (this.m00);
					case 1:
						return (this.m01);
					case 2:
						return (this.m02);
					default:
						break;
				}
				break;
			case 1:
				switch (column) {
					case 0:
						return (this.m10);
					case 1:
						return (this.m11);
					case 2:
						return (this.m12);
					default:
						break;
				}
				break;

			case 2:
				switch (column) {
					case 0:
						return (this.m20);
					case 1:
						return (this.m21);
					case 2:
						return (this.m22);
					default:
						break;
				}
				break;

			default:
				break;
		}
		throw new ArrayIndexOutOfBoundsException(VecMathI18N.getString("Matrix3f5"));
	}

	/**
	 * Sets the specified row of this matrix3f to the three values provided.
	 * 
	 * @param row
	 *          the row number to be modified (zero indexed)
	 * @param x
	 *          the first column element
	 * @param y
	 *          the second column element
	 * @param z
	 *          the third column element
	 */
	public final void setRow(int row, float x, float y, float z) {
		switch (row) {
			case 0:
				this.m00 = x;
				this.m01 = y;
				this.m02 = z;
				break;

			case 1:
				this.m10 = x;
				this.m11 = y;
				this.m12 = z;
				break;

			case 2:
				this.m20 = x;
				this.m21 = y;
				this.m22 = z;
				break;

			default:
				throw new ArrayIndexOutOfBoundsException(VecMathI18N
						.getString("Matrix3f6"));
		}
	}

	/**
	 * Sets the specified row of this matrix3f to the Vector provided.
	 * 
	 * @param row
	 *          the row number to be modified (zero indexed)
	 * @param v
	 *          the replacement row
	 */
	public final void setRow(int row, Vector3f v) {
		switch (row) {
			case 0:
				this.m00 = v.x;
				this.m01 = v.y;
				this.m02 = v.z;
				break;

			case 1:
				this.m10 = v.x;
				this.m11 = v.y;
				this.m12 = v.z;
				break;

			case 2:
				this.m20 = v.x;
				this.m21 = v.y;
				this.m22 = v.z;
				break;

			default:
				throw new ArrayIndexOutOfBoundsException(VecMathI18N
						.getString("Matrix3f6"));
		}
	}

	/**
	 * Sets the specified row of this matrix3f to the three values provided.
	 * 
	 * @param row
	 *          the row number to be modified (zero indexed)
	 * @param v
	 *          the replacement row
	 */
	public final void setRow(int row, float v[]) {
		switch (row) {
			case 0:
				this.m00 = v[0];
				this.m01 = v[1];
				this.m02 = v[2];
				break;

			case 1:
				this.m10 = v[0];
				this.m11 = v[1];
				this.m12 = v[2];
				break;

			case 2:
				this.m20 = v[0];
				this.m21 = v[1];
				this.m22 = v[2];
				break;

			default:
				throw new ArrayIndexOutOfBoundsException(VecMathI18N
						.getString("Matrix3f6"));
		}
	}

	/**
	 * Sets the specified column of this matrix3f to the three values provided.
	 * 
	 * @param column
	 *          the column number to be modified (zero indexed)
	 * @param x
	 *          the first row element
	 * @param y
	 *          the second row element
	 * @param z
	 *          the third row element
	 */
	public final void setColumn(int column, float x, float y, float z) {
		switch (column) {
			case 0:
				this.m00 = x;
				this.m10 = y;
				this.m20 = z;
				break;

			case 1:
				this.m01 = x;
				this.m11 = y;
				this.m21 = z;
				break;

			case 2:
				this.m02 = x;
				this.m12 = y;
				this.m22 = z;
				break;

			default:
				throw new ArrayIndexOutOfBoundsException(VecMathI18N
						.getString("Matrix3f9"));
		}
	}

	/**
	 * Sets the specified column of this matrix3f to the vector provided.
	 * 
	 * @param column
	 *          the column number to be modified (zero indexed)
	 * @param v
	 *          the replacement column
	 */
	public final void setColumn(int column, Vector3f v) {
		switch (column) {
			case 0:
				this.m00 = v.x;
				this.m10 = v.y;
				this.m20 = v.z;
				break;

			case 1:
				this.m01 = v.x;
				this.m11 = v.y;
				this.m21 = v.z;
				break;

			case 2:
				this.m02 = v.x;
				this.m12 = v.y;
				this.m22 = v.z;
				break;

			default:
				throw new ArrayIndexOutOfBoundsException(VecMathI18N
						.getString("Matrix3f9"));
		}
	}

	/**
	 * Sets the specified column of this matrix3f to the three values provided.
	 * 
	 * @param column
	 *          the column number to be modified (zero indexed)
	 * @param v
	 *          the replacement column
	 */
	public final void setColumn(int column, float v[]) {
		switch (column) {
			case 0:
				this.m00 = v[0];
				this.m10 = v[1];
				this.m20 = v[2];
				break;

			case 1:
				this.m01 = v[0];
				this.m11 = v[1];
				this.m21 = v[2];
				break;

			case 2:
				this.m02 = v[0];
				this.m12 = v[1];
				this.m22 = v[2];
				break;

			default:
				throw new ArrayIndexOutOfBoundsException(VecMathI18N
						.getString("Matrix3f9"));
		}
	}

	/**
	 * Performs an SVD normalization of this matrix to calculate and return the
	 * uniform scale factor. If the matrix has non-uniform scale factors, the
	 * largest of the x, y, and z scale factors will be returned. This matrix is
	 * not modified.
	 * 
	 * @return the scale factor of this matrix
	 */
	public final float getScale() {

		double[] tmp_rot = new double[9]; // scratch matrix
		double[] tmp_scale = new double[3]; // scratch matrix
		getScaleRotate(tmp_scale, tmp_rot);

		return ((float) Matrix3d.max3(tmp_scale));

	}

	/**
	 * Adds a scalar to each component of this matrix.
	 * 
	 * @param scalar
	 *          the scalar adder
	 */
	public final void add(float scalar) {
		m00 += scalar;
		m01 += scalar;
		m02 += scalar;
		m10 += scalar;
		m11 += scalar;
		m12 += scalar;
		m20 += scalar;
		m21 += scalar;
		m22 += scalar;
	}

	/**
	 * Adds a scalar to each component of the matrix m1 and places the result into
	 * this. Matrix m1 is not modified.
	 * 
	 * @param scalar
	 *          the scalar adder.
	 * @param m1
	 *          the original matrix values
	 */
	public final void add(float scalar, Matrix3f m1) {
		this.m00 = m1.m00 + scalar;
		this.m01 = m1.m01 + scalar;
		this.m02 = m1.m02 + scalar;
		this.m10 = m1.m10 + scalar;
		this.m11 = m1.m11 + scalar;
		this.m12 = m1.m12 + scalar;
		this.m20 = m1.m20 + scalar;
		this.m21 = m1.m21 + scalar;
		this.m22 = m1.m22 + scalar;
	}

	/**
	 * Sets the value of this matrix to the matrix sum of matrices m1 and m2.
	 * 
	 * @param m1
	 *          the first matrix
	 * @param m2
	 *          the second matrix
	 */
	public final void add(Matrix3f m1, Matrix3f m2) {
		this.m00 = m1.m00 + m2.m00;
		this.m01 = m1.m01 + m2.m01;
		this.m02 = m1.m02 + m2.m02;

		this.m10 = m1.m10 + m2.m10;
		this.m11 = m1.m11 + m2.m11;
		this.m12 = m1.m12 + m2.m12;

		this.m20 = m1.m20 + m2.m20;
		this.m21 = m1.m21 + m2.m21;
		this.m22 = m1.m22 + m2.m22;
	}

	/**
	 * Sets the value of this matrix to the matrix sum of itself and matrix m1.
	 * 
	 * @param m1
	 *          the other matrix
	 */
	public final void add(Matrix3f m1) {
		this.m00 += m1.m00;
		this.m01 += m1.m01;
		this.m02 += m1.m02;

		this.m10 += m1.m10;
		this.m11 += m1.m11;
		this.m12 += m1.m12;

		this.m20 += m1.m20;
		this.m21 += m1.m21;
		this.m22 += m1.m22;
	}

	/**
	 * Sets the value of this matrix to the matrix difference of matrices m1 and
	 * m2.
	 * 
	 * @param m1
	 *          the first matrix
	 * @param m2
	 *          the second matrix
	 */
	public final void sub(Matrix3f m1, Matrix3f m2) {
		this.m00 = m1.m00 - m2.m00;
		this.m01 = m1.m01 - m2.m01;
		this.m02 = m1.m02 - m2.m02;

		this.m10 = m1.m10 - m2.m10;
		this.m11 = m1.m11 - m2.m11;
		this.m12 = m1.m12 - m2.m12;

		this.m20 = m1.m20 - m2.m20;
		this.m21 = m1.m21 - m2.m21;
		this.m22 = m1.m22 - m2.m22;
	}

	/**
	 * Sets the value of this matrix to the matrix difference of itself and matrix
	 * m1 (this = this - m1).
	 * 
	 * @param m1
	 *          the other matrix
	 */
	public final void sub(Matrix3f m1) {
		this.m00 -= m1.m00;
		this.m01 -= m1.m01;
		this.m02 -= m1.m02;

		this.m10 -= m1.m10;
		this.m11 -= m1.m11;
		this.m12 -= m1.m12;

		this.m20 -= m1.m20;
		this.m21 -= m1.m21;
		this.m22 -= m1.m22;
	}

	/**
	 * Sets the value of this matrix to its transpose.
	 */
	public final void transpose() {
		float temp;

		temp = this.m10;
		this.m10 = this.m01;
		this.m01 = temp;

		temp = this.m20;
		this.m20 = this.m02;
		this.m02 = temp;

		temp = this.m21;
		this.m21 = this.m12;
		this.m12 = temp;
	}

	/**
	 * Sets the value of this matrix to the transpose of the argument matrix.
	 * 
	 * @param m1
	 *          the matrix to be transposed
	 */
	public final void transpose(Matrix3f m1) {
		if (this != m1) {
			this.m00 = m1.m00;
			this.m01 = m1.m10;
			this.m02 = m1.m20;

			this.m10 = m1.m01;
			this.m11 = m1.m11;
			this.m12 = m1.m21;

			this.m20 = m1.m02;
			this.m21 = m1.m12;
			this.m22 = m1.m22;
		} else
			this.transpose();
	}

	/**
	 * Sets the value of this matrix to the matrix conversion of the (single
	 * precision) quaternion argument.
	 * 
	 * @param q1
	 *          the quaternion to be converted
	 */
	public final void set(Quat4f q1) {
		this.m00 = 1.0f - 2.0f * q1.y * q1.y - 2.0f * q1.z * q1.z;
		this.m10 = 2.0f * (q1.x * q1.y + q1.w * q1.z);
		this.m20 = 2.0f * (q1.x * q1.z - q1.w * q1.y);

		this.m01 = 2.0f * (q1.x * q1.y - q1.w * q1.z);
		this.m11 = 1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.z * q1.z;
		this.m21 = 2.0f * (q1.y * q1.z + q1.w * q1.x);

		this.m02 = 2.0f * (q1.x * q1.z + q1.w * q1.y);
		this.m12 = 2.0f * (q1.y * q1.z - q1.w * q1.x);
		this.m22 = 1.0f - 2.0f * q1.x * q1.x - 2.0f * q1.y * q1.y;
	}

	/**
	 * Sets the value of this matrix to the matrix conversion of the (single
	 * precision) axis and angle argument.
	 * 
	 * @param a1
	 *          the axis and angle to be converted
	 */
	public final void set(AxisAngle4f a1) {
		float mag = (float) Math.sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
		if (mag < EPS) {
			m00 = 1.0f;
			m01 = 0.0f;
			m02 = 0.0f;

			m10 = 0.0f;
			m11 = 1.0f;
			m12 = 0.0f;

			m20 = 0.0f;
			m21 = 0.0f;
			m22 = 1.0f;
		} else {
			mag = 1.0f / mag;
			float ax = a1.x * mag;
			float ay = a1.y * mag;
			float az = a1.z * mag;

			float sinTheta = (float) Math.sin((float) a1.angle);
			float cosTheta = (float) Math.cos((float) a1.angle);
			float t = (float) 1.0 - cosTheta;

			float xz = ax * az;
			float xy = ax * ay;
			float yz = ay * az;

			m00 = t * ax * ax + cosTheta;
			m01 = t * xy - sinTheta * az;
			m02 = t * xz + sinTheta * ay;

			m10 = t * xy + sinTheta * az;
			m11 = t * ay * ay + cosTheta;
			m12 = t * yz - sinTheta * ax;

			m20 = t * xz - sinTheta * ay;
			m21 = t * yz + sinTheta * ax;
			m22 = t * az * az + cosTheta;
		}

	}

	/**
	 * Sets the value of this matrix to the matrix conversion of the (double
	 * precision) axis and angle argument.
	 * 
	 * @param a1
	 *          the axis and angle to be converted
	 */
	public final void set(AxisAngle4d a1) {
		double mag = Math.sqrt(a1.x * a1.x + a1.y * a1.y + a1.z * a1.z);
		if (mag < EPS) {
			m00 = 1.0f;
			m01 = 0.0f;
			m02 = 0.0f;

			m10 = 0.0f;
			m11 = 1.0f;
			m12 = 0.0f;

			m20 = 0.0f;
			m21 = 0.0f;
			m22 = 1.0f;
		} else {
			mag = 1.0 / mag;
			double ax = a1.x * mag;
			double ay = a1.y * mag;
			double az = a1.z * mag;

			double sinTheta = Math.sin(a1.angle);
			double cosTheta = Math.cos(a1.angle);
			double t = 1.0 - cosTheta;

			double xz = ax * az;
			double xy = ax * ay;
			double yz = ay * az;

			m00 = (float) (t * ax * ax + cosTheta);
			m01 = (float) (t * xy - sinTheta * az);
			m02 = (float) (t * xz + sinTheta * ay);

			m10 = (float) (t * xy + sinTheta * az);
			m11 = (float) (t * ay * ay + cosTheta);
			m12 = (float) (t * yz - sinTheta * ax);

			m20 = (float) (t * xz - sinTheta * ay);
			m21 = (float) (t * yz + sinTheta * ax);
			m22 = (float) (t * az * az + cosTheta);
		}

	}

	/**
	 * Sets the value of this matrix to the matrix conversion of the (single
	 * precision) quaternion argument.
	 * 
	 * @param q1
	 *          the quaternion to be converted
	 */
	public final void set(Quat4d q1) {
		this.m00 = (float) (1.0 - 2.0 * q1.y * q1.y - 2.0 * q1.z * q1.z);
		this.m10 = (float) (2.0 * (q1.x * q1.y + q1.w * q1.z));
		this.m20 = (float) (2.0 * (q1.x * q1.z - q1.w * q1.y));

		this.m01 = (float) (2.0 * (q1.x * q1.y - q1.w * q1.z));
		this.m11 = (float) (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.z * q1.z);
		this.m21 = (float) (2.0 * (q1.y * q1.z + q1.w * q1.x));

		this.m02 = (float) (2.0 * (q1.x * q1.z + q1.w * q1.y));
		this.m12 = (float) (2.0 * (q1.y * q1.z - q1.w * q1.x));
		this.m22 = (float) (1.0 - 2.0 * q1.x * q1.x - 2.0 * q1.y * q1.y);
	}

	/**
	 * Sets the values in this Matrix3f equal to the row-major array parameter
	 * (ie, the first three elements of the array will be copied into the first
	 * row of this matrix, etc.).
	 * 
	 * @param m
	 *          the single precision array of length 9
	 */
	public final void set(float[] m) {
		m00 = m[0];
		m01 = m[1];
		m02 = m[2];

		m10 = m[3];
		m11 = m[4];
		m12 = m[5];

		m20 = m[6];
		m21 = m[7];
		m22 = m[8];

	}

	/**
	 * Sets the value of this matrix to the value of the Matrix3f argument.
	 * 
	 * @param m1
	 *          the source matrix3f
	 */
	public final void set(Matrix3f m1) {

		this.m00 = m1.m00;
		this.m01 = m1.m01;
		this.m02 = m1.m02;

		this.m10 = m1.m10;
		this.m11 = m1.m11;
		this.m12 = m1.m12;

		this.m20 = m1.m20;
		this.m21 = m1.m21;
		this.m22 = m1.m22;

	}

	/**
	 * Sets the value of this matrix to the float value of the Matrix3d argument.
	 * 
	 * @param m1
	 *          the source matrix3d
	 */
	public final void set(Matrix3d m1) {

		this.m00 = (float) m1.m00;
		this.m01 = (float) m1.m01;
		this.m02 = (float) m1.m02;

		this.m10 = (float) m1.m10;
		this.m11 = (float) m1.m11;
		this.m12 = (float) m1.m12;

		this.m20 = (float) m1.m20;
		this.m21 = (float) m1.m21;
		this.m22 = (float) m1.m22;

	}

	/**
	 * Sets the value of this matrix to the matrix inverse of the passed matrix
	 * m1.
	 * 
	 * @param m1
	 *          the matrix to be inverted
	 */
	public final void invert(Matrix3f m1) {
		invertGeneral(m1);
	}

	/**
	 * Inverts this matrix in place.
	 */
	public final void invert() {
		invertGeneral(this);
	}

	/**
	 * General invert routine. Inverts m1 and places the result in "this". Note
	 * that this routine handles both the "this" version and the non-"this"
	 * version.
	 * 
	 * Also note that since this routine is slow anyway, we won't worry about
	 * allocating a little bit of garbage.
	 */
	private final void invertGeneral(Matrix3f m1) {
		double temp[] = new double[9];
		double result[] = new double[9];
		int row_perm[] = new int[3];
		int i;

		// Use LU decomposition and backsubstitution code specifically
		// for floating-point 3x3 matrices.

		// Copy source matrix to t1tmp
		temp[0] = (double) m1.m00;
		temp[1] = (double) m1.m01;
		temp[2] = (double) m1.m02;

		temp[3] = (double) m1.m10;
		temp[4] = (double) m1.m11;
		temp[5] = (double) m1.m12;

		temp[6] = (double) m1.m20;
		temp[7] = (double) m1.m21;
		temp[8] = (double) m1.m22;

		// Calculate LU decomposition: Is the matrix singular?
		if (!luDecomposition(temp, row_perm)) {
			// Matrix has no inverse
			throw new SingularMatrixException(VecMathI18N.getString("Matrix3f12"));
		}

		// Perform back substitution on the identity matrix
		for (i = 0; i < 9; i++)
			result[i] = 0.0;
		result[0] = 1.0;
		result[4] = 1.0;
		result[8] = 1.0;
		luBacksubstitution(temp, row_perm, result);

		this.m00 = (float) result[0];
		this.m01 = (float) result[1];
		this.m02 = (float) result[2];

		this.m10 = (float) result[3];
		this.m11 = (float) result[4];
		this.m12 = (float) result[5];

		this.m20 = (float) result[6];
		this.m21 = (float) result[7];
		this.m22 = (float) result[8];

	}

	/**
	 * Given a 3x3 array "matrix0", this function replaces it with the LU
	 * decomposition of a row-wise permutation of itself. The input parameters are
	 * "matrix0" and "dimen". The array "matrix0" is also an output parameter. The
	 * vector "row_perm[3]" is an output parameter that contains the row
	 * permutations resulting from partial pivoting. The output parameter
	 * "even_row_xchg" is 1 when the number of row exchanges is even, or -1
	 * otherwise. Assumes data type is always double.
	 * 
	 * This function is similar to luDecomposition, except that it is tuned
	 * specifically for 3x3 matrices.
	 * 
	 * @return true if the matrix is nonsingular, or false otherwise.
	 */
	//
	// Reference: Press, Flannery, Teukolsky, Vetterling,
	// _Numerical_Recipes_in_C_, Cambridge University Press,
	// 1988, pp 40-45.
	//
	static boolean luDecomposition(double[] matrix0, int[] row_perm) {

		double row_scale[] = new double[3];

		// Determine implicit scaling information by looping over rows
		{
			int i, j;
			int ptr, rs;
			double big, temp;

			ptr = 0;
			rs = 0;

			// For each row ...
			i = 3;
			while (i-- != 0) {
				big = 0.0;

				// For each column, find the largest element in the row
				j = 3;
				while (j-- != 0) {
					temp = matrix0[ptr++];
					temp = Math.abs(temp);
					if (temp > big) {
						big = temp;
					}
				}

				// Is the matrix singular?
				if (big == 0.0) {
					return false;
				}
				row_scale[rs++] = 1.0 / big;
			}
		}

		{
			int j;
			int mtx;

			mtx = 0;

			// For all columns, execute Crout's method
			for (j = 0; j < 3; j++) {
				int i, imax, k;
				int target, p1, p2;
				double sum, big, temp;

				// Determine elements of upper diagonal matrix U
				for (i = 0; i < j; i++) {
					target = mtx + (3 * i) + j;
					sum = matrix0[target];
					k = i;
					p1 = mtx + (3 * i);
					p2 = mtx + j;
					while (k-- != 0) {
						sum -= matrix0[p1] * matrix0[p2];
						p1++;
						p2 += 3;
					}
					matrix0[target] = sum;
				}

				// Search for largest pivot element and calculate
				// intermediate elements of lower diagonal matrix L.
				big = 0.0;
				imax = -1;
				for (i = j; i < 3; i++) {
					target = mtx + (3 * i) + j;
					sum = matrix0[target];
					k = j;
					p1 = mtx + (3 * i);
					p2 = mtx + j;
					while (k-- != 0) {
						sum -= matrix0[p1] * matrix0[p2];
						p1++;
						p2 += 3;
					}
					matrix0[target] = sum;

					// Is this the best pivot so far?
					if ((temp = row_scale[i] * Math.abs(sum)) >= big) {
						big = temp;
						imax = i;
					}
				}

				if (imax < 0) {
					throw new RuntimeException(VecMathI18N.getString("Matrix3f13"));
				}

				// Is a row exchange necessary?
				if (j != imax) {
					// Yes: exchange rows
					k = 3;
					p1 = mtx + (3 * imax);
					p2 = mtx + (3 * j);
					while (k-- != 0) {
						temp = matrix0[p1];
						matrix0[p1++] = matrix0[p2];
						matrix0[p2++] = temp;
					}

					// Record change in scale factor
					row_scale[imax] = row_scale[j];
				}

				// Record row permutation
				row_perm[j] = imax;

				// Is the matrix singular
				if (matrix0[(mtx + (3 * j) + j)] == 0.0) {
					return false;
				}

				// Divide elements of lower diagonal matrix L by pivot
				if (j != (3 - 1)) {
					temp = 1.0 / (matrix0[(mtx + (3 * j) + j)]);
					target = mtx + (3 * (j + 1)) + j;
					i = 2 - j;
					while (i-- != 0) {
						matrix0[target] *= temp;
						target += 3;
					}
				}
			}
		}

		return true;
	}

	/**
	 * Solves a set of linear equations. The input parameters "matrix1", and
	 * "row_perm" come from luDecompostionD3x3 and do not change here. The
	 * parameter "matrix2" is a set of column vectors assembled into a 3x3 matrix
	 * of floating-point values. The procedure takes each column of "matrix2" in
	 * turn and treats it as the right-hand side of the matrix equation Ax = LUx =
	 * b. The solution vector replaces the original column of the matrix.
	 * 
	 * If "matrix2" is the identity matrix, the procedure replaces its contents
	 * with the inverse of the matrix from which "matrix1" was originally derived.
	 */
	//
	// Reference: Press, Flannery, Teukolsky, Vetterling,
	// _Numerical_Recipes_in_C_, Cambridge University Press,
	// 1988, pp 44-45.
	//
	static void luBacksubstitution(double[] matrix1, int[] row_perm,
			double[] matrix2) {

		int i, ii, ip, j, k;
		int rp;
		int cv, rv;

		// rp = row_perm;
		rp = 0;

		// For each column vector of matrix2 ...
		for (k = 0; k < 3; k++) {
			// cv = &(matrix2[0][k]);
			cv = k;
			ii = -1;

			// Forward substitution
			for (i = 0; i < 3; i++) {
				double sum;

				ip = row_perm[rp + i];
				sum = matrix2[cv + 3 * ip];
				matrix2[cv + 3 * ip] = matrix2[cv + 3 * i];
				if (ii >= 0) {
					// rv = &(matrix1[i][0]);
					rv = i * 3;
					for (j = ii; j <= i - 1; j++) {
						sum -= matrix1[rv + j] * matrix2[cv + 3 * j];
					}
				} else if (sum != 0.0) {
					ii = i;
				}
				matrix2[cv + 3 * i] = sum;
			}

			// Backsubstitution
			// rv = &(matrix1[3][0]);
			rv = 2 * 3;
			matrix2[cv + 3 * 2] /= matrix1[rv + 2];

			rv -= 3;
			matrix2[cv + 3 * 1] = (matrix2[cv + 3 * 1] - matrix1[rv + 2]
					* matrix2[cv + 3 * 2])
					/ matrix1[rv + 1];

			rv -= 3;
			matrix2[cv + 4 * 0] = (matrix2[cv + 3 * 0] - matrix1[rv + 1]
					* matrix2[cv + 3 * 1] - matrix1[rv + 2] * matrix2[cv + 3 * 2])
					/ matrix1[rv + 0];

		}
	}

	/**
	 * Computes the determinant of this matrix.
	 * 
	 * @return the determinant of this matrix
	 */
	public final float determinant() {
		float total;
		total = this.m00 * (this.m11 * this.m22 - this.m12 * this.m21) + this.m01
				* (this.m12 * this.m20 - this.m10 * this.m22) + this.m02
				* (this.m10 * this.m21 - this.m11 * this.m20);
		return total;
	}

	/**
	 * Sets the value of this matrix to a scale matrix with the passed scale
	 * amount.
	 * 
	 * @param scale
	 *          the scale factor for the matrix
	 */
	public final void set(float scale) {
		this.m00 = scale;
		this.m01 = (float) 0.0;
		this.m02 = (float) 0.0;

		this.m10 = (float) 0.0;
		this.m11 = scale;
		this.m12 = (float) 0.0;

		this.m20 = (float) 0.0;
		this.m21 = (float) 0.0;
		this.m22 = scale;
	}

	/**
	 * Sets the value of this matrix to a counter clockwise rotation about the x
	 * axis.
	 * 
	 * @param angle
	 *          the angle to rotate about the X axis in radians
	 */
	public final void rotX(float angle) {
		float sinAngle, cosAngle;

		sinAngle = (float) Math.sin((double) angle);
		cosAngle = (float) Math.cos((double) angle);

		this.m00 = (float) 1.0;
		this.m01 = (float) 0.0;
		this.m02 = (float) 0.0;

		this.m10 = (float) 0.0;
		this.m11 = cosAngle;
		this.m12 = -sinAngle;

		this.m20 = (float) 0.0;
		this.m21 = sinAngle;
		this.m22 = cosAngle;
	}

	/**
	 * Sets the value of this matrix to a counter clockwise rotation about the y
	 * axis.
	 * 
	 * @param angle
	 *          the angle to rotate about the Y axis in radians
	 */
	public final void rotY(float angle) {
		float sinAngle, cosAngle;

		sinAngle = (float) Math.sin((double) angle);
		cosAngle = (float) Math.cos((double) angle);

		this.m00 = cosAngle;
		this.m01 = (float) 0.0;
		this.m02 = sinAngle;

		this.m10 = (float) 0.0;
		this.m11 = (float) 1.0;
		this.m12 = (float) 0.0;

		this.m20 = -sinAngle;
		this.m21 = (float) 0.0;
		this.m22 = cosAngle;
	}

	/**
	 * Sets the value of this matrix to a counter clockwise rotation about the z
	 * axis.
	 * 
	 * @param angle
	 *          the angle to rotate about the Z axis in radians
	 */
	public final void rotZ(float angle) {
		float sinAngle, cosAngle;

		sinAngle = (float) Math.sin((double) angle);
		cosAngle = (float) Math.cos((double) angle);

		this.m00 = cosAngle;
		this.m01 = -sinAngle;
		this.m02 = (float) 0.0;

		this.m10 = sinAngle;
		this.m11 = cosAngle;
		this.m12 = (float) 0.0;

		this.m20 = (float) 0.0;
		this.m21 = (float) 0.0;
		this.m22 = (float) 1.0;
	}

	/**
	 * Multiplies each element of this matrix by a scalar.
	 * 
	 * @param scalar
	 *          the scalar multiplier
	 */
	public final void mul(float scalar) {
		m00 *= scalar;
		m01 *= scalar;
		m02 *= scalar;

		m10 *= scalar;
		m11 *= scalar;
		m12 *= scalar;

		m20 *= scalar;
		m21 *= scalar;
		m22 *= scalar;
	}

	/**
	 * Multiplies each element of matrix m1 by a scalar and places the result into
	 * this. Matrix m1 is not modified.
	 * 
	 * @param scalar
	 *          the scalar multiplier
	 * @param m1
	 *          the original matrix
	 */
	public final void mul(float scalar, Matrix3f m1) {
		this.m00 = scalar * m1.m00;
		this.m01 = scalar * m1.m01;
		this.m02 = scalar * m1.m02;

		this.m10 = scalar * m1.m10;
		this.m11 = scalar * m1.m11;
		this.m12 = scalar * m1.m12;

		this.m20 = scalar * m1.m20;
		this.m21 = scalar * m1.m21;
		this.m22 = scalar * m1.m22;

	}

	/**
	 * Sets the value of this matrix to the result of multiplying itself with
	 * matrix m1.
	 * 
	 * @param m1
	 *          the other matrix
	 */
	public final void mul(Matrix3f m1) {
		float m00, m01, m02, m10, m11, m12, m20, m21, m22;

		m00 = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
		m01 = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
		m02 = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;

		m10 = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
		m11 = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
		m12 = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;

		m20 = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
		m21 = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
		m22 = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;

		this.m00 = m00;
		this.m01 = m01;
		this.m02 = m02;
		this.m10 = m10;
		this.m11 = m11;
		this.m12 = m12;
		this.m20 = m20;
		this.m21 = m21;
		this.m22 = m22;
	}

	/**
	 * Sets the value of this matrix to the result of multiplying the two argument
	 * matrices together.
	 * 
	 * @param m1
	 *          the first matrix
	 * @param m2
	 *          the second matrix
	 */
	public final void mul(Matrix3f m1, Matrix3f m2) {
		if (this != m1 && this != m2) {
			this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
			this.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
			this.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;

			this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
			this.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
			this.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;

			this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
			this.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
			this.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
		} else {
			float m00, m01, m02, m10, m11, m12, m20, m21, m22;

			m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
			m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
			m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;

			m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
			m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
			m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;

			m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
			m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
			m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;

			this.m00 = m00;
			this.m01 = m01;
			this.m02 = m02;
			this.m10 = m10;
			this.m11 = m11;
			this.m12 = m12;
			this.m20 = m20;
			this.m21 = m21;
			this.m22 = m22;
		}
	}

	/**
	 * Multiplies this matrix by matrix m1, does an SVD normalization of the
	 * result, and places the result back into this matrix. this =
	 * SVDnorm(this*m1).
	 * 
	 * @param m1
	 *          the matrix on the right hand side of the multiplication
	 */
	public final void mulNormalize(Matrix3f m1) {

		double[] tmp = new double[9]; // scratch matrix
		double[] tmp_rot = new double[9]; // scratch matrix
		double[] tmp_scale = new double[3]; // scratch matrix

		tmp[0] = this.m00 * m1.m00 + this.m01 * m1.m10 + this.m02 * m1.m20;
		tmp[1] = this.m00 * m1.m01 + this.m01 * m1.m11 + this.m02 * m1.m21;
		tmp[2] = this.m00 * m1.m02 + this.m01 * m1.m12 + this.m02 * m1.m22;

		tmp[3] = this.m10 * m1.m00 + this.m11 * m1.m10 + this.m12 * m1.m20;
		tmp[4] = this.m10 * m1.m01 + this.m11 * m1.m11 + this.m12 * m1.m21;
		tmp[5] = this.m10 * m1.m02 + this.m11 * m1.m12 + this.m12 * m1.m22;

		tmp[6] = this.m20 * m1.m00 + this.m21 * m1.m10 + this.m22 * m1.m20;
		tmp[7] = this.m20 * m1.m01 + this.m21 * m1.m11 + this.m22 * m1.m21;
		tmp[8] = this.m20 * m1.m02 + this.m21 * m1.m12 + this.m22 * m1.m22;

		Matrix3d.compute_svd(tmp, tmp_scale, tmp_rot);

		this.m00 = (float) (tmp_rot[0]);
		this.m01 = (float) (tmp_rot[1]);
		this.m02 = (float) (tmp_rot[2]);

		this.m10 = (float) (tmp_rot[3]);
		this.m11 = (float) (tmp_rot[4]);
		this.m12 = (float) (tmp_rot[5]);

		this.m20 = (float) (tmp_rot[6]);
		this.m21 = (float) (tmp_rot[7]);
		this.m22 = (float) (tmp_rot[8]);

	}

	/**
	 * Multiplies matrix m1 by matrix m2, does an SVD normalization of the result,
	 * and places the result into this matrix. this = SVDnorm(m1*m2).
	 * 
	 * @param m1
	 *          the matrix on the left hand side of the multiplication
	 * @param m2
	 *          the matrix on the right hand side of the multiplication
	 */
	public final void mulNormalize(Matrix3f m1, Matrix3f m2) {

		double[] tmp = new double[9]; // scratch matrix
		double[] tmp_rot = new double[9]; // scratch matrix
		double[] tmp_scale = new double[3]; // scratch matrix

		tmp[0] = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
		tmp[1] = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
		tmp[2] = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;

		tmp[3] = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
		tmp[4] = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
		tmp[5] = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;

		tmp[6] = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
		tmp[7] = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
		tmp[8] = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;

		Matrix3d.compute_svd(tmp, tmp_scale, tmp_rot);

		this.m00 = (float) (tmp_rot[0]);
		this.m01 = (float) (tmp_rot[1]);
		this.m02 = (float) (tmp_rot[2]);

		this.m10 = (float) (tmp_rot[3]);
		this.m11 = (float) (tmp_rot[4]);
		this.m12 = (float) (tmp_rot[5]);

		this.m20 = (float) (tmp_rot[6]);
		this.m21 = (float) (tmp_rot[7]);
		this.m22 = (float) (tmp_rot[8]);
	}

	/**
	 * Multiplies the transpose of matrix m1 times the transpose of matrix m2, and
	 * places the result into this.
	 * 
	 * @param m1
	 *          the matrix on the left hand side of the multiplication
	 * @param m2
	 *          the matrix on the right hand side of the multiplication
	 */
	public final void mulTransposeBoth(Matrix3f m1, Matrix3f m2) {
		if (this != m1 && this != m2) {
			this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02;
			this.m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12;
			this.m02 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22;

			this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02;
			this.m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12;
			this.m12 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22;

			this.m20 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02;
			this.m21 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12;
			this.m22 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22;
		} else {
			float m00, m01, m02, m10, m11, m12, m20, m21, m22; // vars for temp result
																													// matrix

			m00 = m1.m00 * m2.m00 + m1.m10 * m2.m01 + m1.m20 * m2.m02;
			m01 = m1.m00 * m2.m10 + m1.m10 * m2.m11 + m1.m20 * m2.m12;
			m02 = m1.m00 * m2.m20 + m1.m10 * m2.m21 + m1.m20 * m2.m22;

			m10 = m1.m01 * m2.m00 + m1.m11 * m2.m01 + m1.m21 * m2.m02;
			m11 = m1.m01 * m2.m10 + m1.m11 * m2.m11 + m1.m21 * m2.m12;
			m12 = m1.m01 * m2.m20 + m1.m11 * m2.m21 + m1.m21 * m2.m22;

			m20 = m1.m02 * m2.m00 + m1.m12 * m2.m01 + m1.m22 * m2.m02;
			m21 = m1.m02 * m2.m10 + m1.m12 * m2.m11 + m1.m22 * m2.m12;
			m22 = m1.m02 * m2.m20 + m1.m12 * m2.m21 + m1.m22 * m2.m22;

			this.m00 = m00;
			this.m01 = m01;
			this.m02 = m02;
			this.m10 = m10;
			this.m11 = m11;
			this.m12 = m12;
			this.m20 = m20;
			this.m21 = m21;
			this.m22 = m22;
		}

	}

	/**
	 * Multiplies matrix m1 times the transpose of matrix m2, and places the
	 * result into this.
	 * 
	 * @param m1
	 *          the matrix on the left hand side of the multiplication
	 * @param m2
	 *          the matrix on the right hand side of the multiplication
	 */
	public final void mulTransposeRight(Matrix3f m1, Matrix3f m2) {
		if (this != m1 && this != m2) {
			this.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
			this.m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
			this.m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;

			this.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
			this.m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
			this.m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;

			this.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
			this.m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
			this.m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;
		} else {
			float m00, m01, m02, m10, m11, m12, m20, m21, m22; // vars for temp result
																													// matrix

			m00 = m1.m00 * m2.m00 + m1.m01 * m2.m01 + m1.m02 * m2.m02;
			m01 = m1.m00 * m2.m10 + m1.m01 * m2.m11 + m1.m02 * m2.m12;
			m02 = m1.m00 * m2.m20 + m1.m01 * m2.m21 + m1.m02 * m2.m22;

			m10 = m1.m10 * m2.m00 + m1.m11 * m2.m01 + m1.m12 * m2.m02;
			m11 = m1.m10 * m2.m10 + m1.m11 * m2.m11 + m1.m12 * m2.m12;
			m12 = m1.m10 * m2.m20 + m1.m11 * m2.m21 + m1.m12 * m2.m22;

			m20 = m1.m20 * m2.m00 + m1.m21 * m2.m01 + m1.m22 * m2.m02;
			m21 = m1.m20 * m2.m10 + m1.m21 * m2.m11 + m1.m22 * m2.m12;
			m22 = m1.m20 * m2.m20 + m1.m21 * m2.m21 + m1.m22 * m2.m22;

			this.m00 = m00;
			this.m01 = m01;
			this.m02 = m02;
			this.m10 = m10;
			this.m11 = m11;
			this.m12 = m12;
			this.m20 = m20;
			this.m21 = m21;
			this.m22 = m22;
		}
	}

	/**
	 * Multiplies the transpose of matrix m1 times matrix m2, and places the
	 * result into this.
	 * 
	 * @param m1
	 *          the matrix on the left hand side of the multiplication
	 * @param m2
	 *          the matrix on the right hand side of the multiplication
	 */
	public final void mulTransposeLeft(Matrix3f m1, Matrix3f m2) {
		if (this != m1 && this != m2) {
			this.m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20;
			this.m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21;
			this.m02 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22;

			this.m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20;
			this.m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21;
			this.m12 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22;

			this.m20 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20;
			this.m21 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21;
			this.m22 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22;
		} else {
			float m00, m01, m02, m10, m11, m12, m20, m21, m22; // vars for temp result
																													// matrix

			m00 = m1.m00 * m2.m00 + m1.m10 * m2.m10 + m1.m20 * m2.m20;
			m01 = m1.m00 * m2.m01 + m1.m10 * m2.m11 + m1.m20 * m2.m21;
			m02 = m1.m00 * m2.m02 + m1.m10 * m2.m12 + m1.m20 * m2.m22;

			m10 = m1.m01 * m2.m00 + m1.m11 * m2.m10 + m1.m21 * m2.m20;
			m11 = m1.m01 * m2.m01 + m1.m11 * m2.m11 + m1.m21 * m2.m21;
			m12 = m1.m01 * m2.m02 + m1.m11 * m2.m12 + m1.m21 * m2.m22;

			m20 = m1.m02 * m2.m00 + m1.m12 * m2.m10 + m1.m22 * m2.m20;
			m21 = m1.m02 * m2.m01 + m1.m12 * m2.m11 + m1.m22 * m2.m21;
			m22 = m1.m02 * m2.m02 + m1.m12 * m2.m12 + m1.m22 * m2.m22;

			this.m00 = m00;
			this.m01 = m01;
			this.m02 = m02;
			this.m10 = m10;
			this.m11 = m11;
			this.m12 = m12;
			this.m20 = m20;
			this.m21 = m21;
			this.m22 = m22;
		}
	}

	/**
	 * Performs singular value decomposition normalization of this matrix.
	 */
	public final void normalize() {

		double[] tmp_rot = new double[9]; // scratch matrix
		double[] tmp_scale = new double[3]; // scratch matrix
		getScaleRotate(tmp_scale, tmp_rot);

		this.m00 = (float) tmp_rot[0];
		this.m01 = (float) tmp_rot[1];
		this.m02 = (float) tmp_rot[2];

		this.m10 = (float) tmp_rot[3];
		this.m11 = (float) tmp_rot[4];
		this.m12 = (float) tmp_rot[5];

		this.m20 = (float) tmp_rot[6];
		this.m21 = (float) tmp_rot[7];
		this.m22 = (float) tmp_rot[8];

	}

	/**
	 * Perform singular value decomposition normalization of matrix m1 and place
	 * the normalized values into this.
	 * 
	 * @param m1
	 *          the matrix values to be normalized
	 */
	public final void normalize(Matrix3f m1) {
		double[] tmp = new double[9]; // scratch matrix
		double[] tmp_rot = new double[9]; // scratch matrix
		double[] tmp_scale = new double[3]; // scratch matrix

		tmp[0] = m1.m00;
		tmp[1] = m1.m01;
		tmp[2] = m1.m02;

		tmp[3] = m1.m10;
		tmp[4] = m1.m11;
		tmp[5] = m1.m12;

		tmp[6] = m1.m20;
		tmp[7] = m1.m21;
		tmp[8] = m1.m22;

		Matrix3d.compute_svd(tmp, tmp_scale, tmp_rot);

		this.m00 = (float) (tmp_rot[0]);
		this.m01 = (float) (tmp_rot[1]);
		this.m02 = (float) (tmp_rot[2]);

		this.m10 = (float) (tmp_rot[3]);
		this.m11 = (float) (tmp_rot[4]);
		this.m12 = (float) (tmp_rot[5]);

		this.m20 = (float) (tmp_rot[6]);
		this.m21 = (float) (tmp_rot[7]);
		this.m22 = (float) (tmp_rot[8]);

	}

	/**
	 * Perform cross product normalization of this matrix.
	 */
	public final void normalizeCP() {
		float mag = 1.0f / (float) Math.sqrt(m00 * m00 + m10 * m10 + m20 * m20);
		m00 = m00 * mag;
		m10 = m10 * mag;
		m20 = m20 * mag;

		mag = 1.0f / (float) Math.sqrt(m01 * m01 + m11 * m11 + m21 * m21);
		m01 = m01 * mag;
		m11 = m11 * mag;
		m21 = m21 * mag;

		m02 = m10 * m21 - m11 * m20;
		m12 = m01 * m20 - m00 * m21;
		m22 = m00 * m11 - m01 * m10;

	}

	/**
	 * Perform cross product normalization of matrix m1 and place the normalized
	 * values into this.
	 * 
	 * @param m1
	 *          Provides the matrix values to be normalized
	 */
	public final void normalizeCP(Matrix3f m1) {
		float mag = 1.0f / (float) Math.sqrt(m1.m00 * m1.m00 + m1.m10 * m1.m10
				+ m1.m20 * m1.m20);
		m00 = m1.m00 * mag;
		m10 = m1.m10 * mag;
		m20 = m1.m20 * mag;

		mag = 1.0f / (float) Math.sqrt(m1.m01 * m1.m01 + m1.m11 * m1.m11 + m1.m21
				* m1.m21);
		m01 = m1.m01 * mag;
		m11 = m1.m11 * mag;
		m21 = m1.m21 * mag;

		m02 = m10 * m21 - m11 * m20;
		m12 = m01 * m20 - m00 * m21;
		m22 = m00 * m11 - m01 * m10;

	}

	/**
	 * Returns true if all of the data members of Matrix3f m1 are equal to the
	 * corresponding data members in this Matrix3f.
	 * 
	 * @param m1
	 *          the matrix with which the comparison is made
	 * @return true or false
	 */
	public boolean equals(Matrix3f m1) {
		try {

			return (this.m00 == m1.m00 && this.m01 == m1.m01 && this.m02 == m1.m02
					&& this.m10 == m1.m10 && this.m11 == m1.m11 && this.m12 == m1.m12
					&& this.m20 == m1.m20 && this.m21 == m1.m21 && this.m22 == m1.m22);
		} catch (NullPointerException e2) {
			return false;
		}

	}

	/**
	 * Returns true if the Object o1 is of type Matrix3f and all of the data
	 * members of o1 are equal to the corresponding data members in this Matrix3f.
	 * 
	 * @param o1
	 *          the object with which the comparison is made
	 * @return true or false
	 */
	public boolean equals(Object o1) {
		try {

			Matrix3f m2 = (Matrix3f) o1;
			return (this.m00 == m2.m00 && this.m01 == m2.m01 && this.m02 == m2.m02
					&& this.m10 == m2.m10 && this.m11 == m2.m11 && this.m12 == m2.m12
					&& this.m20 == m2.m20 && this.m21 == m2.m21 && this.m22 == m2.m22);
		} catch (ClassCastException e1) {
			return false;
		} catch (NullPointerException e2) {
			return false;
		}
	}

	/**
	 * Returns true if the L-infinite distance between this matrix and matrix m1
	 * is less than or equal to the epsilon parameter, otherwise returns false.
	 * The L-infinite distance is equal to MAX[i=0,1,2 ; j=0,1,2 ; abs(this.m(i,j)
	 * - m1.m(i,j)]
	 * 
	 * @param m1
	 *          the matrix to be compared to this matrix
	 * @param epsilon
	 *          the threshold value
	 */
	public boolean epsilonEquals(Matrix3f m1, float epsilon) {
		boolean status = true;

		if (Math.abs(this.m00 - m1.m00) > epsilon)
			status = false;
		if (Math.abs(this.m01 - m1.m01) > epsilon)
			status = false;
		if (Math.abs(this.m02 - m1.m02) > epsilon)
			status = false;

		if (Math.abs(this.m10 - m1.m10) > epsilon)
			status = false;
		if (Math.abs(this.m11 - m1.m11) > epsilon)
			status = false;
		if (Math.abs(this.m12 - m1.m12) > epsilon)
			status = false;

		if (Math.abs(this.m20 - m1.m20) > epsilon)
			status = false;
		if (Math.abs(this.m21 - m1.m21) > epsilon)
			status = false;
		if (Math.abs(this.m22 - m1.m22) > epsilon)
			status = false;

		return (status);

	}

	/**
	 * Returns a hash code value based on the data values in this object. Two
	 * different Matrix3f objects with identical data values (i.e.,
	 * Matrix3f.equals returns true) will return the same hash code value. Two
	 * objects with different data members may return the same hash value,
	 * although this is not likely.
	 * 
	 * @return the integer hash code value
	 */
	public int hashCode() {
		long bits = 1L;
		bits = 31L * bits + VecMathUtil.floatToIntBits(m00);
		bits = 31L * bits + VecMathUtil.floatToIntBits(m01);
		bits = 31L * bits + VecMathUtil.floatToIntBits(m02);
		bits = 31L * bits + VecMathUtil.floatToIntBits(m10);
		bits = 31L * bits + VecMathUtil.floatToIntBits(m11);
		bits = 31L * bits + (long) VecMathUtil.floatToIntBits(m12);
		bits = 31L * bits + (long) VecMathUtil.floatToIntBits(m20);
		bits = 31L * bits + (long) VecMathUtil.floatToIntBits(m21);
		bits = 31L * bits + (long) VecMathUtil.floatToIntBits(m22);
		return (int) (bits ^ (bits >> 32));
	}

	/**
	 * Sets this matrix to all zeros.
	 */
	public final void setZero() {
		m00 = 0.0f;
		m01 = 0.0f;
		m02 = 0.0f;

		m10 = 0.0f;
		m11 = 0.0f;
		m12 = 0.0f;

		m20 = 0.0f;
		m21 = 0.0f;
		m22 = 0.0f;

	}

	/**
	 * Negates the value of this matrix: this = -this.
	 */
	public final void negate() {
		this.m00 = -this.m00;
		this.m01 = -this.m01;
		this.m02 = -this.m02;

		this.m10 = -this.m10;
		this.m11 = -this.m11;
		this.m12 = -this.m12;

		this.m20 = -this.m20;
		this.m21 = -this.m21;
		this.m22 = -this.m22;

	}

	/**
	 * Sets the value of this matrix equal to the negation of of the Matrix3f
	 * parameter.
	 * 
	 * @param m1
	 *          the source matrix
	 */
	public final void negate(Matrix3f m1) {
		this.m00 = -m1.m00;
		this.m01 = -m1.m01;
		this.m02 = -m1.m02;

		this.m10 = -m1.m10;
		this.m11 = -m1.m11;
		this.m12 = -m1.m12;

		this.m20 = -m1.m20;
		this.m21 = -m1.m21;
		this.m22 = -m1.m22;

	}

	/**
	 * Multiply this matrix by the tuple t and place the result back into the
	 * tuple (t = this*t).
	 * 
	 * @param t
	 *          the tuple to be multiplied by this matrix and then replaced
	 */
	public final void transform(Tuple3f t) {
		float x, y, z;
		x = m00 * t.x + m01 * t.y + m02 * t.z;
		y = m10 * t.x + m11 * t.y + m12 * t.z;
		z = m20 * t.x + m21 * t.y + m22 * t.z;
		t.set(x, y, z);
	}

	/**
	 * Multiply this matrix by the tuple t and and place the result into the tuple
	 * "result" (result = this*t).
	 * 
	 * @param t
	 *          the tuple to be multiplied by this matrix
	 * @param result
	 *          the tuple into which the product is placed
	 */
	public final void transform(Tuple3f t, Tuple3f result) {
		float x, y;
		x = m00 * t.x + m01 * t.y + m02 * t.z;
		y = m10 * t.x + m11 * t.y + m12 * t.z;
		result.z = m20 * t.x + m21 * t.y + m22 * t.z;
		result.x = x;
		result.y = y;
	}

	/**
	 * perform SVD (if necessary to get rotational component
	 */
	void getScaleRotate(double[] scales, double[] rot) {

		double[] tmp = new double[9]; // scratch matrix
		tmp[0] = m00;
		tmp[1] = m01;
		tmp[2] = m02;
		tmp[3] = m10;
		tmp[4] = m11;
		tmp[5] = m12;
		tmp[6] = m20;
		tmp[7] = m21;
		tmp[8] = m22;
		Matrix3d.compute_svd(tmp, scales, rot);

		return;

	}

	/**
	 * Get the first matrix element in the first row.
	 * 
	 * @return Returns the m00.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM00() {
		return m00;
	}

	/**
	 * Set the first matrix element in the first row.
	 * 
	 * @param m00
	 *          The m00 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM00(float m00) {
		this.m00 = m00;
	}

	/**
	 * Get the second matrix element in the first row.
	 * 
	 * @return Returns the m01.
	 * 
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM01() {
		return m01;
	}

	/**
	 * Set the second matrix element in the first row.
	 * 
	 * @param m01
	 *          The m01 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM01(float m01) {
		this.m01 = m01;
	}

	/**
	 * Get the third matrix element in the first row.
	 * 
	 * @return Returns the m02.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM02() {
		return m02;
	}

	/**
	 * Set the third matrix element in the first row.
	 * 
	 * @param m02
	 *          The m02 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM02(float m02) {
		this.m02 = m02;
	}

	/**
	 * Get first matrix element in the second row.
	 * 
	 * @return Returns the m10.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM10() {
		return m10;
	}

	/**
	 * Set first matrix element in the second row.
	 * 
	 * @param m10
	 *          The m10 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM10(float m10) {
		this.m10 = m10;
	}

	/**
	 * Get second matrix element in the second row.
	 * 
	 * @return Returns the m11.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM11() {
		return m11;
	}

	/**
	 * Set the second matrix element in the second row.
	 * 
	 * @param m11
	 *          The m11 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM11(float m11) {
		this.m11 = m11;
	}

	/**
	 * Get the third matrix element in the second row.
	 * 
	 * @return Returns the m12.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM12() {
		return m12;
	}

	/**
	 * Set the third matrix element in the second row.
	 * 
	 * @param m12
	 *          The m12 to set.
	 * @since vecmath 1.5
	 */
	public final void setM12(float m12) {
		this.m12 = m12;
	}

	/**
	 * Get the first matrix element in the third row.
	 * 
	 * @return Returns the m20.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM20() {
		return m20;
	}

	/**
	 * Set the first matrix element in the third row.
	 * 
	 * @param m20
	 *          The m20 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM20(float m20) {
		this.m20 = m20;
	}

	/**
	 * Get the second matrix element in the third row.
	 * 
	 * @return Returns the m21.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM21() {
		return m21;
	}

	/**
	 * Set the second matrix element in the third row.
	 * 
	 * @param m21
	 *          The m21 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM21(float m21) {
		this.m21 = m21;
	}

	/**
	 * Get the third matrix element in the third row .
	 * 
	 * @return Returns the m22.
	 * 
	 * @since vecmath 1.5
	 */
	public final float getM22() {
		return m22;
	}

	/**
	 * Set the third matrix element in the third row.
	 * 
	 * @param m22
	 *          The m22 to set.
	 * 
	 * @since vecmath 1.5
	 */
	public final void setM22(float m22) {
		this.m22 = m22;
	}
		
	/**
	 * Return the matrix under array form
	 */
	public float[] asArray()
	{
		return new float[] { 
				m00, m01, m02,
				m10, m11, m12,
				m20, m21, m22
				};
	}

}
